#ifndef GTRACK_INT_H__
#define GTRACK_INT_H__
// system
#include <stdint.h>
// local
#include "gtrack_list.h"
#include "gtrack.h"
#include "gtrack_queue.h"
#include "track_unitConfidence.h"

#define PI      3.14159265358979323846f
#define RAD(x) ((x) * PI / 180.f)
#define DEG(x) ((x) * 180.f / PI)

/**
    * @brief
    *  3x3矩阵定义
    *
    * @details
    *  该结构描述了 3x3 矩阵，可将其视为按行排列的数组或按单个元素编址的数组。
    */
typedef union
{
    float a[9];
    struct
    {
        float e11;
        float e12;
        float e13;
        float e21;
        float e22;
        float e23;
        float e31;
        float e32;
        float e33;
    };
} MATRIX3x3;

/**
    * @brief
    *  4x4矩阵定义
    *
    * @details
    *  该结构描述了 4x4 矩阵，可将其视为按行排列的数组或按单个元素编址的数组。
    */
typedef union
{
    float a[16];
    struct
    {
        float e11;
        float e12;
        float e13;
        float e14;
        float e21;
        float e22;
        float e23;
        float e24;
        float e31;
        float e32;
        float e33;
        float e34;
        float e41;
        float e42;
        float e43;
        float e44;
    };
} MATRIX4x4;

/**
    * @brief
    *  6x6矩阵定义
    *
    * @details
    *  该结构描述了 6x6 矩阵，可将其视为行向数组或按单个元素寻址。
    */
typedef union
{
    float a[36];
    struct
    {
        float e11;
        float e12;
        float e13;
        float e14;
        float e15;
        float e16;
        float e21;
        float e22;
        float e23;
        float e24;
        float e25;
        float e26;
        float e31;
        float e32;
        float e33;
        float e34;
        float e35;
        float e36;
        float e41;
        float e42;
        float e43;
        float e44;
        float e45;
        float e46;
        float e51;
        float e52;
        float e53;
        float e54;
        float e55;
        float e56;
        float e61;
        float e62;
        float e63;
        float e64;
        float e65;
        float e66;
    };
} MATRIX6x6;

/**
    * @brief
    *  GTRACK 单元参数结构体
    *
    * @details
    *  该结构描述了用于实例化 GTRACK 单元实例的参数
    */
typedef struct
{
    /**  @brief  跟踪单元标识符 (UID) */
    uint8_t uid;
    /**  @brief 所用的状态向量类型 */
    GTRACK_STATE_VECTOR_TYPE stateVectorType;
    /**  @brief 配置的目标最大加速度, m/s2 */
    float maxAcceleration[3];
    /**  @brief 检测时的预期目标径向速度, m/s */
    float initialRadialVelocity;

    /**  @brief  雷达参数 */
    GTRACK_RadarParams radarParams;
    /**  @brief  门限参数 */
    GTRACK_gatingParams gatingParams;
    /**  @brief  分配参数 */
    GTRACK_allocationParams allocationParams;
    /**  @brief  展开(Unrolling) 参数 */
    GTRACK_unrollingParams unrollingParams;
    /**  @brief  轨迹合并参数 */
    GTRACK_mergeParams mergeParams;
    /**  @brief  场景参数 */
    GTRACK_sceneryParams sceneryParams;
    /**  @brief 当前状态转移矩阵的指针 */
    float* F;
    /**  @brief 当前过程噪声矩阵的指针 */
    float* Q;
} TrackingParams;

/**
    * @brief
    *  GTRACK单元速度处理状态
    *
    * @details
    *  该结构描述了 GTRACK 速度处理状态
    */
typedef enum
{
    /**  @brief INIT */
    VELOCITY_INIT = 0,
    /**  @brief Range Rate filtering 距离变化率过滤? */
    VELOCITY_RATE_FILTER,
    /**  @brief 稳定速度估计 */
    VELOCITY_TRACKING,
    /**  @brief 锁定 */
    VELOCITY_LOCKED
} VelocityHandlingState;

/**
* @brief
*  GTRACK 单元实例结构
*
* @details
*  该结构描述了单个 GTRACK 单元实例
*/
typedef struct
{
    /**  @brief 跟踪单元标识符 */
    uint8_t uid;
    /**  @brief 目标标识符 */
    uint16_t tid;

    /**  @brief 时间戳 */
    uint64_t heartBeatCount;
    /**  @brief 分配的时间 */
    uint64_t allocationTime;
    /**  @brief 分配的距离 */
    float allocationRange;
    /**  @brief 分配的径向速度 */
    float allocationVelocity;
    /**  @brief 分配时种群的总SNR */
    float allocSNR;
    /**  @brief Estimated Number of Points */
    float estNumOfPoints;
    /**  @brief 当前状态 */
    TrackState state;
    /**  @brief 指示目标当前是否为静态 */
    bool isTargetStatic;

    /**  @brief 请求的状态向量类型 */
    GTRACK_STATE_VECTOR_TYPE stateVectorType;
    /**  @brief 当前状态向量类型  */
    GTRACK_STATE_VECTOR_TYPE currentStateVectorType;
    /**  @brief 状态矢量中各维度的长度，例如 2 表示 XY，3 表示 XYZ */
    uint16_t stateVectorDimNum;
    /**  @brief 状态矢量中的维数，例如 2 表示恒定速度，3 表示恒定加速度 */
    uint16_t stateVectorDimLength;
    /**  @brief 状态向量的长度 */
    uint16_t stateVectorLength;
    /**  @brief 测量向量的长度 */
    uint16_t measurementVectorLength;
    /**  @brief Veboseness Mask */
    uint64_t verbose;

    /**  @brief 雷达参数 */
    GTRACK_RadarParams* radarParams;
    /**  @brief 门限参数 */
    GTRACK_gatingParams* gatingParams;
    /**  @brief 分配参数 */
    GTRACK_allocationParams* allocationParams;
    /**  @brief 展开(Unrolling)参数 */
    GTRACK_unrollingParams* unrollingParams;
    /**  @brief Scenery参数 */
    GTRACK_sceneryParams* sceneryParams;
    /**  @brief 当前速度处理状态 */
    VelocityHandlingState velocityHandling;
    /**  @brief   预期最大目标加速度 (X，Y，Z)方向, m/s2 */
    float maxAcceleration[3];
    /**  @brief 检测时的预期目标径向速度, m/s */
    float initialRadialVelocity;
    /**  @brief Current Range Rate value 当前距离变化率? */
    float rangeRate;

    /**  @brief Detection状态到active状态的计数 */
    uint16_t detect2activeCount;
    /**  @brief Detection状态到free状态的计数 */
    uint16_t detect2freeCount;
    /**  @brief active状态到free状态的计数 */
    uint16_t active2freeCount;

    /** 待合并计数 */
    uint8_t mergedCount[GTRACK_NUM_TRACKS_MAX];

    /**  @brief 当前状态转移矩阵的指针 */
    float* F;
    /**  @brief 当前过程噪声矩阵的指针 */
    float* Q;

    /**  @brief 状态矩阵, estimated 估计值 */
    float S_hat[GTRACK_STATE_VECTOR_SIZE];
    /**  @brief 状态矩阵, predicted 预测值 */
    float S_apriori_hat[GTRACK_STATE_VECTOR_SIZE];
    /**  @brief 状态矩阵, saved 保存至 */
    float S_apriori_saved[GTRACK_STATE_VECTOR_SIZE];
    /**  @brief 过程协方差矩阵, estimated 估计值 */
    float P_hat[GTRACK_STATE_VECTOR_SIZE * GTRACK_STATE_VECTOR_SIZE];
    /**  @brief 过程协方差矩阵, predicted 预测值 */
    float P_apriori_hat[GTRACK_STATE_VECTOR_SIZE * GTRACK_STATE_VECTOR_SIZE];
    /**  @brief 预期测量矩阵 */
    GTRACK_measurementUnion H_s;
    /**  @brief 待关联的预测值的约束 */
    GTRACK_measurementUnion H_limits;
    /**  @brief Estimated spread of the measurements 测量值的估计范围，弃用 */
    GTRACK_measurementUnion estSpread;
    /** @brief 轨迹质心 */
    GTRACK_measurementPoint center;
    /**  @brief 目标的估计物理尺寸?，暂未使用 */
    float estDim[GTRACK_MEASUREMENT_VECTOR_SIZE];
    /**  @brief Group 跟踪离散矩阵 */
    float gD[GTRACK_MEASUREMENT_VECTOR_SIZE * GTRACK_MEASUREMENT_VECTOR_SIZE];
    /**  @brief Group 协方差矩阵（测量Group中的组员与组中心点之间） */
    float gC[GTRACK_MEASUREMENT_VECTOR_SIZE * GTRACK_MEASUREMENT_VECTOR_SIZE];
    /**  @brief Group 协方差矩阵的逆矩阵 */
    float gC_inv[GTRACK_MEASUREMENT_VECTOR_SIZE * GTRACK_MEASUREMENT_VECTOR_SIZE];
    /**  @brief Group 协方差矩阵的行列式 */
    float gC_det;
    /**  @brief 关联函数中使用的增益 */
    float G;
    /**  @brief 评分因子，暂未使用 */
    float sFactor;

    TrackedObject track_state;

} GtrackUnitInstance;

/**
    * @brief
    *  GTRACK 模块实例结构
    *
    * @details
    *  该结构描述了 GTRACK 模块实例
    */
typedef struct
{
    /**  @brief 每帧最大测量点数量 */
    uint16_t maxNumPoints;
    /**  @brief 跟踪对象的最大数量 */
    uint16_t maxNumTracks;
    /**  @brief 跟踪单元参数  */
    TrackingParams params;
    /**  @brief 时间戳  */
    uint64_t heartBeat;
    /**  @brief 记录最佳得分的数组 */
    float* bestScore;
    /**  @brief 记录最佳得分者数组（UIDs） */
    uint8_t* bestIndex;
    /**  @brief 记录每个点的最佳eps */
    float* epsIndex;
    /**  @brief 存储分配点的索引的临时数组 */
    uint16_t* allocIndex;
    /**  @brief Tracking数组的句柄 */
    void** hTrack;
    /**  @brief 当前活动Tracking的链表 (UIDs) */
    GTrack_ListObj activeList;
    /**  @brief 空闲Tracking的链表 (UIDs) */
    GTrack_ListObj freeList;
    /**  @brief UID元素的数组 */
    GTrack_ListElem* uidElem;
    /*   @brief 轨迹所关联的点之索引 */
    uint16_t** associateDatas;
    /*   @brief 轨迹所关联的点的个数 */
    uint16_t* associateNums;
    /*   @brief 点云聚类所需的队列结构 */
    Queue clusterQ;
    /**  @brief 目标标识符的数组 */
    GTRACK_targetDesc* targetDesc;
    /**  @brief 已跟踪目标的个数 */
    uint16_t targetNumTotal;
    /**  @brief 当前正在跟踪的目标的个数 */
    uint16_t targetNumCurrent;

} GtrackModuleInstance;

/**
 *  @b Description
 *  @n
 *      这是一个模块级点云过滤函数。该函数由外部step函数调用，以执行对输入点云的过滤操作。
 *
 *  @param[in]  inst
 *      GTRACK模块实例的指针
 *  @param[in]  point
 *      点云指针
 *  @parma[in]  mNum
 *      点云数量
 *  @retval
 *      None
 */
void gtrack_modulePreprocess(GtrackModuleInstance* inst, GTRACK_measurementPoint* point, uint16_t mNum);

/**
 *  @b Description
 *  @n
 *      这是一个模块级预测函数。该函数由外部step函数调用，以执行单元级卡尔曼滤波预测。
 *
 *  @param[in]  inst
 *      GTRACK模块实例的指针
 *  @retval
 *      None
 */
void gtrack_modulePredict(GtrackModuleInstance* inst);

/**
 *  @b Description
 *  @n
 *      这是一个模块级聚类函数。关联函数调用该函数，用于将点云聚类
 *
 *  @param[in]  inst
 *      GTRACK模块实例的指针
 *  @param[in]  point
 *      输入测量值数组的指针，每个测量值都包含距离/角度/径向速度信息。
 *  @param[in]  num
 *      输入测量值的个数
 *  @param[in]  initIdx
 *      聚类初始点索引
 *  @param[out]  sum
 *      聚类点测量数据之和
 *  @retval
 *      聚类点数
 */
uint16_t gtrack_moduleCluster(GtrackModuleInstance* inst, GTRACK_measurementPoint* point, uint16_t mNum, uint16_t initIdx, GTRACK_measurementPoint* sum);

/**
 *  @b Description
 *  @n
 *      这是一个模块级关联函数。外部step函数调用该函数，将测量点与已知目标关联起来
 *
 *  @param[in]  inst
 *      GTRACK模块实例的指针
 *  @param[in]  point
 *      输入测量值数组的指针，每个测量值都包含距离/角度/径向速度信息。
 *  @param[in]  num
 *      输入测量值的个数
 *  @retval
 *      None
 */
void gtrack_moduleAssociate(GtrackModuleInstance* inst, GTRACK_measurementPoint* point, uint16_t num);

/**
 *  @b Description
 *  @n
 *      这是一个模块级关联函数。外部step函数调用该函数，将测量点与已知目标关联起来
 *
 *  @param[in]  inst
 *      GTRACK模块实例的指针
 *  @param[in]  point
 *      输入测量值数组的指针，每个测量值都包含距离/角度/径向速度信息。
 *  @param[in] center
 *      聚类中心
 *  @param[in]  num
 *      该聚类中测量值的个数
 *  @retval
 *      None
 */
void gtrack_moduleScore(GtrackModuleInstance* inst, GTRACK_measurementPoint* point, GTRACK_measurementPoint center, uint16_t num);

/**
 *  @b Description
 *  @n
 *      这是一个模块级分配函数。外部step函数调用该函数为非关联测量点分配新目标。
 *
 *  @param[in]  inst
 *      GTRACK模块实例的指针
 *  @param[in]  point
 *      输入测量值数组的指针，每个测量值都包含距离/角度/径向速度信息。
 *  @param[in]  num
 *      输入测量值的个数
 *  @param[in]  center
 *      群组质心
 *   @param[in] allocNum
 *      群组观测点的个数
 *  @retval
 *      None
 */
void gtrack_moduleAllocate(GtrackModuleInstance* inst, GTRACK_measurementPoint* point, GTRACK_measurementPoint center, uint16_t allocNum);

/**
 *  @b Description
 *  @n
 *      这是一个模块级更新函数。该函数由外部step函数调用，以执行单元级卡尔曼滤波器更新。
 *
 *  @param[in]  inst
 *      GTRACK模块实例的指针
 *  @param[in]  point
 *      输入测量值数组的指针，每个测量值都包含距离/角度/径向速度信息。
 *  @param[in]  var
 *      输入测量误差数组的指针。如果变量未知，则设为 NULL。
 *  @param[in]  num
 *      测量值的个数
 *  @retval
 *      None
 */
void gtrack_moduleUpdate(GtrackModuleInstance* inst, GTRACK_measurementPoint* point, uint16_t num);

/**
 * @b Description
 * @n
 *      这是一个模块级轨迹合并函数。该函数由外部step函数调用，以执行轨迹合并。
 * @param[in] inst
 *     GTRACK模块实例的指针
 */
void gtrack_moduleMerge(GtrackModuleInstance* inst);

void gtrack_moduleDelete(GtrackModuleInstance* inst);

void gtrack_moduleCalcAdaptiveEps(GtrackModuleInstance* inst, GTRACK_measurementPoint* points, uint16_t num_points, uint16_t k_neighbors);

/**
 *  @b Description
 *  @n
 *      这是一个模块级报告函数。该函数由外部step函数调用，以获取单元级数据。
 *
 *  @param[in]  inst
 *      GTRACK模块实例的指针
 *  @param[out]  t
 *      指向 GTRACK_targetDesc 数组的指针。
 *      该函数为每个被跟踪的目标填充描述信息
 *  @param[out]  tNum
 *      指向 uint16_t 值的指针。
 *      函数返回已填充目标描述符的数量
 *  @retval
 *      None
 */
void gtrack_moduleReport(GtrackModuleInstance* inst, GTRACK_targetDesc* t, uint16_t* tNum, GTRACK_measurementPoint* point, uint16_t mNum);
/**
 *  @b Description
 *  @n
 *		GTRACK 模块调用该函数，使用所需的配置参数实例化 GTRACK 单元。
 *		函数返回一个句柄，模块使用该句柄调用单元的方法。
 *
 *  @param[in]  params
 *		这是一个指向配置结构的指针。该结构包含 GTRACK 算法暴露的所有参数。
 *		配置无需持续存在。
 *  @param[out] errCode
 *      出错时弹出的错误代码，参见 ref GTRACK_ERROR_CODE。
 *  @retval
 *     GTRACK单元的句柄
 */
void*      gtrack_unitCreate(TrackingParams* params, int32_t* errCode);

/**
 *  @b Description
 *  @n
 *		GTRACK 模块调用该函数删除 GTRACK 单元实例
 *
 *  @param[in]  handle
 *      GTRACK 单元的句柄
 *  @retval
 *      None
 */
void       gtrack_unitDelete(void* handle);

/**
 *  @b Description
 *  @n
 *      GTRACK 模块调用该函数启动目标跟踪。在模块分配步骤中，一旦新的点集通过分配阈值，就会调用此函数。
 *
 *  @param[in]  handle
 *		GTRACK 单元的句柄
 *  @param[in]  timeStamp
 *		allocation分配时间戳
 *  @param[in]  tid
 *		这是给GTRACK单元的目标标识符
 *  @param[in]  u
 *		这是一个指向测量坐标中中心点的指针
 *  @retval
 *      None
 */
void       gtrack_unitStart(void* handle, uint64_t timeStamp, uint32_t tid, GTRACK_measurement_vector* u, float allocSNR);

/**
*  @b Description
*  @n
*		GTRACK 模块调用该函数停止目标跟踪。当跟踪状态转换为 “空闲”（FREE）时，该函数被调用。
*
*  @param[in]  handle
*		GTRACK 单元的句柄
*  @retval
*      None
*/
void       gtrack_unitStop(void* handle);

/**
 *  @b Description
 *  @n
 *		GTRACK 模块调用该函数来运行 GTRACK 单元预测步骤
 *
 *  @param[in]  handle
 *		GTRACK 单元的句柄
 *  @retval
 *      None
 */
void       gtrack_unitPredict(void* handle);

/**
 *  @b Description
 *  @n
 *		GTRACK 模块调用该函数来执行跟踪单元的更新步骤。
 *
 *  @param[in]  handle
 *		GTRACK 单元的句柄
 *  @param[in]  point
 *		测量点数组
 *  @param[in]  pBestScore
 *      最佳得分数组的的指针。
 *  @param[in]  pAssociate
 *		一个存储该轨迹所关联的点的索引的队列的指针。
 *  @param[out] isUnique
 *      这是一个数组，表示点是否属于单一目标（1）或不属于单一目标（0）。
 *  @param[in]  num
 *		测量点的个数
 *  @retval
 *      None
 */
void gtrack_unitUpdate(void* handle, GTRACK_measurementPoint* point, float* pBestScore, uint16_t* pAssociate, uint16_t num);

/**
 *  @b Description
 *  @n
 *		GTRACK 模块调用该函数从 GTRACK 单元获取测量向量评分
 *
 *  @param[in]  handle
 *		GTRACK 单元的句柄
 *  @param[in]  point
 *		测量点数组
 *  @param[in]  allocIndex
 *      存储属于该聚类类别的点之索引的数组
 *  @param[inout]  bestScore
 *		最新分数表的指针，包含最佳分数。只更新较好的分数。
 *  @param[inout]  bestInd
 *		当前分数获胜者的指针。只有得分较高的获胜者才会被更新
 *  @param[in]  num
 *		 测量点的个数
 *  @retval
 *      None
 */
void       gtrack_unitScore(void* handle, GTRACK_measurementPoint* point, uint16_t* allocIndex, float* bestScore, uint8_t* bestInd,GTRACK_measurementPoint center, uint16_t num);

/**
 *  @b Description
 *  @n
 *		GTRACK 模块调用该函数来运行 GTRACK 单元级状态机
 *
 *  @param[in]  handle
 *		GTRACK 单元的句柄
 *  @param[in]  num
 *		已关联的测量的数量
 *  @retval
 *      None
 */
void       gtrack_unitEvent(void* handle, uint16_t num);

/**
 *  @b Description
 *  @n
 *		GTRACK 模块调用该函数向目标描述符报告 GTRACK 单元结果。
 *
 *  @param[in]  handle
 *		GTRACK 单元的句柄
 *  @param[in]  target
 *		这是指向目标描述符的指针。
 *  @retval
 *      None
 */
void       gtrack_unitReport(void* handle, GTRACK_targetDesc* target);

/**
 *  @b Description
 *  @n
 *		GTRACK 模块调用该函数来获取目标预期测量矩阵 H_s。
 *
 *  @param[in]  handle
 *		GTRACK 单元的句柄
 *  @param[in]  pH
 *		这是指向测量数组 H 的指针，由本函数填充。
 *  @retval
 *      None
 */
void       gtrack_unitGetH(void* handle, float* H);

/**
 * @b  Description
 * @n
 *      GTRACK 模块调用该函数来获取目标观测噪声矩阵 R。
 * 
 * @param[in] handle
 *      GTRACK 单元的句柄
 * @param[in] center
 *      轨迹质心
 * @param[out] R
 *      带计算目标的观测噪声矩阵 R
 * @ retval
 *      None
 */
void       gtrack_unitGetR(void* handle, GTRACK_measurementPoint center, float* R);

/**
 * @b   Description
 * @n
 *       GTRACK 模块调用该函数来计算目标后验状态向量S_hat。
 * @param[in] K
 *      卡尔曼增益
 * @param[in] y
 *      预测与观测的残差
 * @param[in] S_apriori
 *      预测的先验状态向量
 * @param[out] S_hat
 *      后验状态向量
 */
void       gtrack_uintGetSHat(float* K, float* y, float* S_apriori, float* S_hat);

/**
 * @b   Description
 * @n
 *       GTRACK 模块调用该函数来计算目标后验过程噪声矩阵P_hat。
 * @param[in] K
 *      卡尔曼增益
 * @param[in] PJ
 *      先验过程噪声矩阵与雅各比矩阵的乘积
 * @param[in] P_apriori
 *      预测的先验过程噪声矩阵
 * @param[out] P_hat
 *      后验过程噪声矩阵
 */
void       gtrack_uintGetPHat(float* K, float* PJ, float* P_apriori, float* P_hat);

/**
 * @b   Description
 * @n
 *       GTRACK 模块调用该函数来计算群组质心观测值的误差协方差矩阵Rc。
 * @param[in] Rm
 *      雷达观测噪声矩阵
 * @param[in] gD
 *      群组离散矩阵
 * @param[in] alpha
 *      群组离散矩阵的权重因子
 * @param[out] Rc
 *      群组质心观测值的误差协方差矩阵
 */
void       gtrack_uintGetRc(float* Rm, float* gD, float alpha, float* Rc);

/**
 * @b   Description
 * @n
 *       GTRACK 模块调用该函数来计算群组残差协方差矩阵gC。
 * @param[in] JPJ
 *      表征群组质心的不确定性的矩阵
 * @param[in] Rm
 *      群组质心观测值的误差协方差矩阵
 * @param[in] gD
 *      群组离散矩阵
 * @param[out] gC
 *      群组残差协方差矩阵
 */
void       gtrack_uintGetgC(float* JPJ, float* Rm, float* gD, float* gC);

/**
 *  @b Description
 *  @n
 *		GTRACK 模块根据当前速度处理状态计算当前速度。
 *
 *  @param[in]  handle
 *		GTRACK 单元的句柄
 *  @param[in]  v
 *		这是指向待计算点的指针
 *  @retval
 *      None
 */
void  gtrack_velocityStateHandling(void* handle, GTRACK_measurement_vector* v);

/**
 *  @b Description
 *  @n
 *		该函数用于根据期望速度值从 +/- rvMax 形式展开径向速度
 *
 *  @param[in]  rvMax
 *		最大的径向速度
 *  @param[in]  rvExp
 *		预期径向速度值
 *  @param[in] rvIn
 *		测量的径向速度值
 *  @retval
 *      rvOut 展开的径向速度值
 */
float gtrack_unrollRadialVelocity(float rvMax, float rvExp, float rvIn);

/**
 *  @b Description
 *  @n
 *		该函数用于将向量量从极坐标转换为笛卡尔坐标
 *
 *  @param[in]  format
 *		所使用的模型
 *  @param[in]  sph
 *		测量（极坐标）向量指针
 *  @param[in]  cart
 *		状态向量指针（笛卡尔坐标）
 *  @retval
 *      None
 */
void  gtrack_spherical2cartesian(GTRACK_STATE_VECTOR_TYPE format, float* sph, float* cart);

/**
 *  @b Description
 *  @n
 *		该函数用于将向量从笛卡尔坐标转换为极坐标。
 *
 *  @param[in]  format
 *		所使用的模型
 *  @param[in]  cart
 *		状态向量指针（笛卡尔坐标）
 *  @param[in] sph
 *		测量（极坐标）向量指针
 *  @retval
 *      None
 */
void  gtrack_cartesian2spherical(GTRACK_STATE_VECTOR_TYPE format, float* cart, float* sph);

/**
 *  @b Description
 *  @n
 *		该函数计算向量的偏导
 *
 *  @param[in]  format
 *		所使用的模型
 *  @param[in]  cart
 *		状态向量指针 (笛卡尔坐标)
 *  @param[in] jac
 *		雅各比偏导向量的指针
 *  @retval
 *      None
 */
void  gtrack_computeJacobian(GTRACK_STATE_VECTOR_TYPE format, float* cart, float* jac);

/**
 *  @b Description
 *  @n
 *		该函数用于将向量从极坐标转换为笛卡尔坐标
 *
 *  @param[in]  v
 *		测量向量的指针（极坐标）
 *  @param[out]  c
 *		状态向量的指针（笛卡尔坐标）
 *  @retval
 *      None
 */
void  gtrack_sph2cart(GTRACK_measurement_vector* v, GTRACK_cartesian_position* c);

/**
 *  @b Description
 *  @n
 *		该函数用于计算在测量坐标（极坐标）下的两点之间的距离
 *
 *  @param[in]  p1
 *		测量向量的指针（极坐标）
 *  @param[in]  p2
 *		测量向量的指针（极坐标）
 *  @retval
 *      distance, m
 */
float gtrack_calcDistance(GTRACK_measurement_vector* v1, GTRACK_measurement_vector* v2);

/**
 *  @b Description
 *  @n
 *		该函数根据估计的测量范围和中心点范围计算目标尺寸估计值？
 *		矩阵为实数，单精度浮点运算。
 *		矩阵为row-major顺序。
 *
 *  @param[in]  mSpread
 *		Vector S
 *  @param[in]  R
 *		scalar Range
 *  @param[out]  tDim
 *  @retval
 *      None
 */
void  gtrack_calcDim(float* mSpread, float R, float* tDim);

/**
 *  @b Description
 *  @n
 *		该函数根据当前目标位置和门限约束计算预测值与测量值的约束条件
 *
 *  @param[in]  range
 *		目标距离（range）
 *  @param[in]  gate_limits
 *		门限约束
 *  @param[out]  limits
 *		预测值与测量值的约束条件
 *  @retval
 *      None
 */
void  gtrack_calcMeasurementLimits(float range, GTRACK_gateLimits* gate_limits, GTRACK_measurement_vector* limits);

#ifdef GRACK_3D
void  gtrack_calcMeasurementVar(GTRACK_measurement_vector* v, GTRACK_varParams* varP, GTRACK_measurement_vector* var);
#endif // GRACK_3D

/**
 *  @b Description
 *  @n
 *		该函数检查点是否在方框边界内
 *
 *  @param[in]  c
 *		在笛卡尔坐标系中的位置的指针
 *  @param[in] box
 *		指向二维方框对象的指针
 *  @retval
 *      1 if inside, 0 otherwise
 */
uint8_t gtrack_isPointInsideBox(GTRACK_cartesian_position* c, GTRACK_boundaryBox* box);

/**
 *  @b Description
 *  @n
 *		该函数用于计算角度的正弦和余弦（单位：度）
 *
 *  @param[in]  theta
 *		角度, in degrees
 *		Matrix B
 *  @param[out] pSinVal
 *		指向计算出的sin的指针, degrees
 *  @param[out] pCosVal
 *		指向计算出的cos的指针, degrees
 *  @retval
 *      无
 */
void gtrack_sincosd(float theta, float* pSinVal, float* pCosVal);

/**
 *  @b Description
 *  @n
 *		该函数用于将矩阵初始化为一个值
 *
 *  @param[in]  rows
 *		矩阵行的数量
 *  @param[in]  cols
 *		矩阵列的数量
 *  @param[in]  value
 *		待设置值
 *  @param[out] A
 *		Matrix A(rows,cols) = ones(rows,cols)*value
 *  @retval
 *      None
 */
void gtrack_matrixInit(uint16_t rows, uint16_t cols, float value, float* A);

/**
 *  @b Description
 *  @n
 *		该函数用于创建单位矩阵
 *
 *  @param[in]  size
 *		单位矩阵的尺寸
 *  @param[out] A
 *		Matrix A(size,size) = eye(size)
 *  @retval
 *      None
 */
void gtrack_matrixEye(uint16_t size, float* A);

/**
 *  @b Description
 *  @n
 *		该函数用于创建对角方形矩阵
 *
 *  @param[in]  size
 *		方阵的尺寸
 *  @param[in]  v
 *		对角向量
 *  @param[out] A
 *		Matrix A(size,size) = diag(v(size))
 *  @retval
 *      None
 */
void gtrack_matrixSetDiag(uint16_t size, float* v, float* A);

/**
 *  @b Description
 *  @n
 *		该函数用于从方阵中获取对角线
 *
 *  @param[in]  size
 *		方阵的尺寸
 *  @param[in] A
 *		Matrix A(size,size)
 *  @param[out]  v
 *		Diagonal vector, v(size) = diag(A(size*size))
 *  @retval
 *      None
 */
void gtrack_matrixGetDiag(uint16_t size, float* A, float* v);

/**
 *  @b Description
 *  @n
 *		该函数用于将向量初始化为一个值
 *
 *  @param[in]  size
 *		向量大小
 *  @param[in]  value
 *		待设置值
 *  @param[out]  A
 *		Vector A
 *
 *       A(size) = ones(size,1)*value
 *  @retval
 *      None
 */
void gtrack_vectorInit(uint16_t size, float value, float* A);

/**
 *  @b Description
 *  @n
 *		该函数将两个向量相加。
 *		向量是实数，单精度浮点运算。
 *
 *  @param[in]  size
 *		向量大小
 *  @param[in]  A
 *		Vector A
 *  @param[in]  B
 *		Vector B
 *  @param[out]  C
 *		Vector C = A + B;
 *  @retval
 *      None
 */
void gtrack_vectorAdd(uint16_t size, float* A, float* B, float* C);

/**
 *  @b Description
 *  @n
 *		该函数将两个向量相减。
 *		向量是实数，单精度浮点运算。
 *
 *  @param[in]  size
 *		向量大小
 *  @param[in]  A
 *		Vector A
 *  @param[in]  B
 *		Vector B
 *  @param[out]  C
 *		Vector C = A - B;
 *  @retval
 *      None
 */
void gtrack_vectorSub(uint16_t size, float* A, float* B, float* C);

/**
 *  @b Description
 *  @n
 *		此函数将向量与标量相乘。
 *		向量是实数，单精度浮点运算。
 *		标量是实数，单精度浮点数。
 *
 *  @param[in]  size
 *		向量大小
 *  @param[in]  A
 *		Vector A
 *  @param[in]  c
 *		Scalar c
 *  @param[out]  B
 *		Vector B = A*c;
 *  @retval
 *      None
 */
void gtrack_vectorScalarMul(uint16_t size, float* A, float c, float* B);

/**
 *  @b Description
 *  @n
 *		该函数执行向量乘以标量的运算，并将结果累加。
 *		向量是实数，单精度浮点运算。
 *		标量是实数，单精度浮点数。
 *
 *  @param[in]  size
 *		向量大小
 *  @param[in]  A
 *		Vector A
 *  @param[in]  c
 *		Scalar c
 *  @param[in, out]  B
 *		Vector B = B + A*c;
 *  @retval
 *      None
 */
void gtrack_vectorScalarMulAcc(uint16_t size, float* A, float c, float* B);

/**
 *  @b Description
 *  @n
 *		该函数执行 IIR 向量滤波
 *		向量是实数，单精度浮点运算。
 *		Alpha 是实数，单精度浮点数。
 *
 *  @param[in]  size
 *		向量大小
 *  @param[in, out]  A
 *		Vector A
 *  @param[in]  alpha
 *		新信息加权系数, (0<=alpha<=1.0f)
 *  @param[in]  B
 *		New information vector B
 *
 *		Vector A = A*(1.0f-alpha) + B*alpha;
 *  @retval
 *      None
 */
void gtrack_vectorFilter(uint16_t size, float* A, float alpha, float* B);

/**
 *  @b Description
 *  @n
 *		该函数根据输入的方差向量和均值向量生成协方差矩阵。
 *		矩阵都是实数，单精度浮点运算。
 *		向量是实数，单精度浮点运算。
 *
 *  @param[in]  size
 *		方阵的大小
 *  @param[in]  A
 *		Matrix A
 *  @param[in]  v
 *		方差向量
 *  @param[in]  mean
 *		均值向量
 *  @retval
 *      None
 */
void gtrack_matrixCovAcc(uint16_t size, float* A, float* v, float* mean);

/**
 *  @b Description
 *  @n
 *		该函数归一化协方差矩阵。
 *		矩阵都是实数，单精度浮点运算。
 *
 *  @param[in]  size
 *		方阵的大小
 *  @param[in,out]  A
 *		Matrix A
 *  @param[in]  num
 *		总数
 *
 *  \ingroup GTRACK_ALG_MATH_FUNCTION
 *
 *  @retval
 *      None
 */
void gtrack_matrixCovNormalize(uint16_t size, float* A, uint16_t num);

/**
 *  @b Description
 *  @n
 *		该函数过滤协方差矩阵
 *		矩阵都是实数，单精度浮点运算。
 *
 *  @param[in]  size
 *		方阵的大小
 *  @param[in,out]  A
 *		Matrix A
 *  @param[in]  B
 *		Matrix B
 *  @param[in]  alpha
 *		滤波系数
 *      Matrix A = (1-alpha)*A + alpha*B
 *  @retval
 *      None
 */

void gtrack_matrixCovFilter(uint16_t size, float* A, float* B, float alpha);

/**
 *  @b Description
 *  @n
 *		该函数用于两个矩阵的乘法运算。
 *		矩阵都是实数，单精度浮点运算。
 *		矩阵是row-major顺序
 *
 *  @param[in]  rows
 *		外维度，行数
 *  @param[in]  m
 *		内维度
 *  @param[in]  cols
 *		外维度，列数
 *  @param[in]  A
 *		Matrix A
 *  @param[in]  B
 *		Matrix B
 *  @param[out]  C
 *		Matrix C(rows,cols) = A(rows, m) X B(m, cols)
 *  @retval
 *      None
 */
void gtrack_matrixMultiply(uint16_t rows, uint16_t m, uint16_t cols, float* A, float* B, float* C);

/**
 *  @b Description
 *  @n
 *		该函数用于两个矩阵的乘法运算。
 *		第一个矩阵 P 的大小为 6x6，第二个矩阵 P 的大小为 3x6。
 *		第二个矩阵先被转置。
 *		矩阵都是实数，单精度浮点运算。
 *		矩阵是row-major顺序。
 *
 *  @param[in]  P
 *		Matrix P
 *  @param[in]  J
 *		Matrix J
 *  @param[out]  PJ
 *		Matrix PJ = P(6,6) X J(3,6)T
 *  @retval
 *      None
 */
void gtrack_matrixComputePJT(float* P, float* J, float* PJ);

/**
 *  @b Description
 *  @n
 *      该函数用于两个矩阵的乘法运算。第二个矩阵会被第一个矩阵转置，所有矩阵都是实数，单精度浮点数。
 *		矩阵是row-major顺序。
 *
 *  @param[in]  rows
 *		外维度，行数
 *  @param[in]  m
 *		内维度
 *  @param[in]  cols
 *		外维度，列数
 *  @param[in]  A
 *		Matrix A
 *  @param[in]  B
 *		Matrix B
 *  @param[out]  C
 *		Matrix C(rows,cols) = A(rows,m) X B(cols,m)T
 *  @retval
 *      None
 */
void gtrack_matrixTransposeMultiply(uint16_t rows, uint16_t m, uint16_t cols, float* A, float* B, float* C);

/**
 *  @b Description
 *  @n
 *		该函数用于将矩阵与标量相乘。
 *		矩阵都是实数，单精度浮点运算。
 *		矩阵是row-major顺序。
 *
 *  @param[in]  rows
 *		行数
 *  @param[in]  cols
 *		列数
 *  @param[in]  A
 *		Matrix A
 *  @param[in]  c
 *		Scalar c
 *  @param[out]  B
 *		Matrix B(rows,cols) = A(rows,cols) * c
 *  @retval
 *      None
 */
void gtrack_matrixScalarMultiply(uint16_t m1, uint16_t m2, float* A, float c, float* B);

/**
 *  @b Description
 *  @n
 *		此函数用于两个矩阵相加。
 *		矩阵都是实数，单精度浮点运算。
 *		矩阵是row-major顺序。
 *
 *  @param[in]  rows
 *		行数
 *  @param[in]  cols
 *		列数
 *  @param[in]  A
 *		Matrix A
 *  @param[in]  B
 *		Matrix B
 *  @param[out]  C
 *		Matrix C(rows,cols) = A(rows,cols) + B(rows,cols)
 *  @retval
 *      None
 */
void gtrack_matrixAdd(uint16_t m1, uint16_t m2, float* A, float* B, float* C);

/**
 *  @b Description
 *  @n
 *		此函数用于两个矩阵相减。
 *		矩阵都是实数，单精度浮点运算。
 *		矩阵是row-major顺序。
 *
 *  @param[in]  rows
 *		行数
 *  @param[in]  cols
 *		列数
 *  @param[in]  A
 *		Matrix A
 *  @param[in]  B
 *		Matrix B
 *  @param[out]  C
 *		Matrix C(rows,cols) = A(rows,cols) - B(rows,cols)
 *  @retval
 *      None
 */
void gtrack_matrixSub(uint16_t m1, uint16_t m2, float* A, float* B, float* C);

/**
 *  @b Description
 *  @n
 *		此函数计算 3x3 矩阵的逆。
 *		矩阵为实数，单精度浮点运算。
 *		矩阵是row-major顺序。
 *
 *  @param[in]  A
 *		Matrix A
 *  @param[out]  det_out
 *		det_out = determinant;
 *  @param[out]  inv
 *		inv = inverse(A);
 *  @retval
 *      None
 */
void gtrack_matrixInv(const float* A, float* det, float* Ainv);

/**
 *  @b Description
 *  @n
 *		该函数用于通过平均非对角元素来强制矩阵对称 矩阵为方阵、实数、单精度浮点数。
 *		矩阵是row-major顺序。
 *
 *  @param[in]  m (m=rows=cols)
 *		方阵的大小
 *  @param[in]  A
 *		Matrix A
 *  @param[out]  B
 *		Matrix B
 *  @retval
 *      None
 */
void gtrack_matrixMakeSymmetrical(uint16_t m, float* A, float* B);

/**
 *  @b Description
 *  @n
 *		该函数对方形矩阵进行 cholesky 分解。
 *		矩阵都是实数，单精度浮点运算。
 *		矩阵是row-major顺序。
 *
 *  @param[in]  dim (dim=rows=cols)
 *		方阵的大小
 *  @param[in]  A
 *		Matrix A
 *  @param[out]  G
 *		Matrix G = cholseky(A);
 *  @retval
 *      None
 */
void gtrack_matrixCholesky(uint16_t dim, const float* A, float* G);

/**
 *  @b Description
 *  @n
 *		该函数计算向量 v 与分布 D 之间的Mahanalobis距离。
 *		向量长度为 3。 向量表示分布中心点和测量向量之间的变化量。
 *		D是 3x3 矩阵。D表示误差协方差矩阵的逆。
 *		向量是实数，单精度浮点数组。
 *		矩阵是实数，单精度浮点数组。
 *		矩阵是row-major顺序。
 *
 *  @param[in]  v
 *		Vector v
 *  @param[in]  D
 *		Matrix D
 *  @param[out]  md
 *		md is computed 3 dimensional mahanalobis distance
 *		md = v*D*v';
 *  @retval
 *      None
 */
void gtrack_computeMahalanobis(float* d, float* S, float* md);

/**
 *  @b Description
 *  @n
 *		该函数计算向量 v 与分布 D 之间的部分Mahanalobis距离。
 *		向量长度为 3。 向量表示分布D中心点和测量向量之间的变化量。
 *		最后一个向量维数被忽略。
 *		D是 3x3 矩阵。D表示误差协方差矩阵的逆。
 *		向量是实数，单精度浮点数组。
 *		矩阵是实数，单精度浮点数组。
 *		矩阵是row-major顺序。
 *
 *  @param[in]  v
 *		Vector v
 *  @param[in]  D
 *		Matrix D
 *  @param[out]  mdp
 *		md is computed 2 dimensional mahanalobis distance (ignoring third dimension, i.e v[2] = 0)
 *		v[2]=0; mdp = v*D*v';
 *  @retval
 *      None
 */
void gtrack_computeMahalanobisPartial(float* v, float* D, float* mdp);


void *gtarck_main_pthread(void *arg);


#endif /* GTRACK_INT_H__ */